#pragma once
#ifndef MULTI_ROBOT_PRM_H
#define MULTI_ROBOT_PRM_H

#include "basic_typedef.h"
#include "Graph.h"
#include "Kd_tree_d.h"
#include "Sampler.h"
#include "CollisionDetector.h"
#include "MultiRobotCollisionDetector.h"
#include "MultiRobotSampler.h"

struct cmpPoint_d {
    bool operator()(const Point_d& a, const Point_d& b) const {
        return a.Ptr() > b.Ptr();
    }
};

class MultiRobotPRM
{
public:
    MultiRobotPRM(vector<Polygon_2> &robot_polygons, int robot_num, Obstacles &obstacles, vector<Conf> &start_confs, vector<Conf> &target_confs, Room &room);
    MultiRobotPRM(vector<Polygon_2> &robot_polygons, int robot_num, Obstacles &obstacles, vector<Conf> &start_confs, vector<Conf> &target_confs, Room &room, MultiRobotCollisionDetector *mr_collision_detector);
    ~MultiRobotPRM();
    void generate_roadmap(int number_vertices = 300, int k_nearest = 12, double eps = 1);
    void update_path(vector<vector<Conf>> &path);
    vector<Conf> generate_valid_sample();
    
private:
    vector<Polygon_2>           &m_robot_polygons;
    int                         m_robot_num;
    Obstacles                   &m_obstacles;           // polygonal obstacles + scenario boundaries
    vector<Conf>                &m_start_confs, &m_target_confs;
    Room                        &m_room;

    Graph<Point_d, cmpPoint_d>  m_graph;                // Graph structure
    Kd_tree_d<Kernel_d>         m_kd_tree;              // Kd-tree for nearest neighbor search

    MultiRobotCollisionDetector* m_mr_collision_detector;
    MultiRobotSampler           m_mr_sampler;
    bool                        m_sould_clear;

    vector<vector<Conf>>        			m_vertices; // Contains all the points in the configuration space
    map<vector<Conf>, Point_d>              m_expandMap;// Maps between a point in the configuration space to the 2d dimention equivelent
    map<Point_d, vector<Conf>, cmpPoint_d>  m_map;      // Reverse maping...
};


#endif